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1.  Introduction 


Position  and  attitude  parameter  estimation  is  often  a  prerequisite  for  controlling  aerospace 
mechanical  systems  moving  in  space  (1 ).  Because  projectile  position  often  cannot  be  accurately 
estimated  a  priori  due  to  various  hardware  and  environmental  uncertainties,  guidance,  navigation 
and  control  (GNC)  systems  are  deployed  onboard  the  object. 

Rocket-propelled  missile  GNC  systems  are  quite  mature  (2,  3),  while  effective  GNC  systems  for 
accurate  microunmanned  vehicles  are  under  development.  For  accurate,  low-cost,  and  real-time 
applications,  the  design  and  implementation  of  the  GNC  systems  are  constrained  by  several 
technical  challenges.  Cost  is  by  far  the  most  challenging  requirement,  while  computational 
efficiency  is  also  required.  In  addition,  robustness  is  needed  in  some  cases  for  the  desired  GNC 
system  to  survive  extremely  harsh  environments,  such  as  gun-launched  projectiles.  Small, 
compact,  and  energy-efficient  structures  are  also  required  for  certain  applications. 

The  primary  function  of  position  estimation  is  to  locate  the  coordinates  of  an  object  with  respect 
to  a  set  of  reference  points  ( 4 ).  In  general,  position  estimation  algorithms  can  be  classified  into 
three  groups:  methods  based  on  time  of  arrival  (TOA),  time  difference  of  arrival  (TDOA),  and 
angle  of  arrival  (AO A).  TOA  methods  capture  the  traveling  time  of  signals  between  the  object 
and  the  reference  points,  from  which  the  distances  between  them  are  determined.  Subsequently, 
the  position  of  the  object  is  estimated  by  the  geometric  relationship  (5,  6).  In  order  to  measure 
the  traveling  time  of  signals,  TOA  systems  need  the  accurate  synchronization  between  the  object 
and  the  reference  points.  TDOA  methods  determine  the  position  of  the  object  by  examining  the 
difference  in  time  at  which  the  signal  arrives  at  multiple  reference  points  ( 4 ).  Thus  TDOA 
systems  require  the  accurate  synchronization  between  the  reference  points.  In  addition,  since  the 
electromagnetic  waves  propagate  at  the  constant  speed  of  light,  both  TOA  and  TDOA  systems 
demand  a  very  high  sampling  rate  to  obtain  high  resolution  of  the  time  arrival.  Similar  to  a 
global  positioning  system  (GPS),  the  requirements  of  accurate  synchronization  and  high 
sampling  rate  make  TOA  and  TDOA  systems  expensive.  AOA  positioning,  on  the  other  hand, 
allows  low-cost  implementation.  It  determines  the  position  of  an  object  by  the  use  of  the 
estimated  angles  between  the  object  and  a  set  of  reference  points  (7-9).  A  variety  of  algorithms 
have  been  proposed  to  estimate  the  angle  of  arrival  ( 10-1 7). 

The  primary  function  of  the  attitude  estimation  system  is  to  determine  the  relative  orientation 
between  the  coordinate  frame  fixed  to  the  object  and  another  reference  coordinate  frame  (18). 
The  attitude  of  the  object  can  be  described  in  three  ways:  direction  cosine  matrix  (DCM), 
quaternion  of  rotation,  and  Euler  angles  (19).  Wahba  (20)  first  addressed  the  problem  of 
determining  the  attitude  of  a  rotating  body  by  the  vector-matching  method.  Subsequently, 
attitude  estimation  methods  have  relied  mostly  on  the  DCM  (21-23).  When  the  DCM 
description  is  used,  the  attitude  parameters  are  linearly  related  to  the  observed  vectors.  Thus 
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linear  estimation  theory  can  be  applied  to  the  estimation  of  the  DCM.  DCM,  however,  uses  nine 
parameters  to  describe  the  attitude,  which  increases  computational  complexity.  The  quaternion 
of  rotation  method  applies  Euler’s  theorem,  which  states  that  any  real  rotation  of  one  coordinate 
frame  with  respect  to  another  may  be  described  by  a  rotation  through  some  angle  about  a  single 
fixed  axis  (24).  The  quaternion  method  reduces  the  number  of  attitude  parameters  from  nine  to 
four.  However,  the  quaternion  parameters  are  nonlinearly  related  to  the  observed  vectors.  Some 
nonlinear  approaches  were  proposed  to  determine  the  quaternion  parameters,  such  as  the  Quest 
algorithm  and  quaternion  Kalman  filters  (25-27).  The  third  approach  of  attitude  estimation  uses 
Euler  angles.  The  use  of  Euler  angles  is  advantageous  since  the  steering  commands  are  explicit 
functions  of  the  object’s  Euler  angles.  In  addition,  the  Euler  angle  method  of  attitude  estimation 
requires  determining  only  three  parameters.  Euler  angles,  however,  are  nonlinearly  related  to  the 
observed  vectors,  and  the  attitude  tracking  using  Euler  angles  is  perhaps  hampered  by  possible 
singularities  (28,  29).  Recently,  Wilson  (30)  developed  a  novel  nonlinear  approach  to  estimate 
the  Euler  angles  of  the  projectile.  However,  this  proposed  system  needs  three  expensive  angular 
rate  sensors.  Subsequently,  Wilson  (37)  extended  this  work  and  designed  an  attitude  estimation 
system  only  based  on  a  set  of  magnetometers.  This  approach,  however,  requires  the  exact 
knowledge  of  the  object’s  dynamic  model  with  properly  modeled  forces  and  moments  that 
sometimes  are  not  observed. 

This  report  proposes  a  position  and  attitude  navigation  system,  where  both  position  and  attitude 
can  be  iteratively  estimated  based  on  small  and  inexpensive  sensor  devices,  such  as  uniform 
linear  arrays  (ULAs)  and  a  magnetometer.  It  will  be  explained  in  section  2.4  that  the  position  or 
attitude  of  the  object  cannot  be  estimated  alone  based  on  N  beacons  or  a  magnetometer — both 
are  required.  In  addition,  the  algorithm  for  simultaneously  estimating  the  position  and  attitude  is 
not  robust.  Therefore,  an  iterative  position  and  attitude  estimation  algorithm  is  developed  in  this 
report.  In  this  iterative  algorithm,  the  position  is  determined  by  AOA  vector  observations,  and 
attitude  is  tracked  using  the  Euler  angle  method. 

The  system  is  accomplished  in  several  steps.  A  set  of  ULAs  on  the  object  receiving  the 
impinging  RF  signals  transmitted  from  ground-based  beacons  is  used  to  estimate  the  directions 
of  arrival  of  the  signals.  In  addition,  since  the  signal-to-noise  ratio  (SNR)  of  the  magnetometer  is 
usually  high,  an  aiding  magnetometer  is  equipped  on  the  spin  axis  of  the  object  to  improve  the 
performance  of  the  position  and  attitude  estimation.  The  ULA  and  magnetometer  measurements 
are  exploited  to  estimate  the  position  of  the  object  based  on  the  prior  knowledge  of  the  initial 
attitude  parameters.  Based  on  the  position  estimation  results,  the  attitude  parameters  are 
estimated  and  updated.  The  resulting  attitude  parameter  estimates  are  used  to  approximate  the 
position  parameters  in  the  next  estimation  cycle.  The  equations  in  the  algorithms  have  been 
developed  assuming  an  ideal  vertical  magnetic  field  vector,  therefore  additional  work  in  the  form 
of  coordinate  transforms  would  be  required  for  a  generalized  solution,  which  is  not  specifically 
addressed  in  this  report.  The  proposed  system  is  low-cost,  and  the  estimates  are  simple  and  thus 
useful  when  GPS  and  expensive  inertial  measurement  units  (IMUs)  are  not  available. 
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The  report  is  organized  as  follows.  The  problem  formulation  is  discussed  in  section  2.  The 
position  estimation  based  on  three-dimensional  (3-D)  AOA  location  algorithm  is  developed  in 
section  3.  The  attitude  estimation  algorithm  based  on  two-dimensional  (2-D)  direction  of  arrival 
(DO  A)  estimation  is  developed  in  section  4.  Simulations  of  the  proposed  algorithms  are 
presented  in  section  5.  Section  6  provides  conclusions. 


2.  Problem  Formulation 


2.1  Coordinate  Systems 

The  motion  of  an  object  is  usually  described  by  rigid  body  equations  of  motion  derived  from 
Newton’s  laws  (29).  This  section  summarizes  and  notates  three  kinds  of  coordinate  systems. 

The  first  is  the  Earth-fixed  coordinate  system,  which  is  fixed  to  the  Earth  with  a  flat  Earth 
assumption.  Denote  X,  Y,  and  Z  as  the  unit  vectors  pointing  in  the  directions  of  the  X,  Y,  and  Z 
axes,  respectively.  Without  loss  of  generality,  the  X,  Y,  and  Z  axes  point  to  forward,  right,  and 
down,  respectively.  The  second  is  the  body-fixed  coordinate  system,  with  three  unit  vectors  Xb, 
Yb,  and  Zb  pointing  to  the  X/„  Y/„  and  Z*  axes,  respectively.  The  A,  axis  is  along  the  object’s 
symmetric  axis,  referred  to  as  the  spin  axis.  The  other  two  axes  are  perpendicular  to  the  spin  axis 
and  each  other.  The  Earth-fixed  coordinate  system  and  the  body-fixed  coordinate  system  are 
shown  in  figure  1 .  The  third  is  the  fixed-plane  system,  which  is  a  body-fixed  system  that  does 
not  take  the  spin  into  account.  The  three  axes  are  the  Xp,  Yp,  and  Zp  axes,  where  Xp  is  overlapped 
with  the  Xb  axis,  and  the  Yp  axis  lies  in  the  horizontal  with  respect  to  the  Earth. 


Figure  1.  Earth-  and  body-fixed  coordinate  systems  and 
the  Euler  angle  rotations. 
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The  Earth-  and  body-fixed  coordinate  systems  are  related  by  a  Euler  rotation  sequence.  The 
Earth-fixed  system  is  first  rotated  about  the  Z  axis  through  the  yaw  angle  y/,  then  about  the  new 
Y  axis  through  the  pitch  angle  6,  finally  about  the  new  X  axis  though  the  roll  angle  (p,  which  is 
shown  in  figure  1.  From  the  Earth-fixed  to  body-fixed  system,  the  transformation  matrix  is 

r  \ 
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where  s.  is  sin(»)  and  c.  is  cos(»).  If  the  roll  angle  is  fixed  as  cp  =  0,  the  transformation  matrix 
from  the  Earth-fixed  to  fixed-plane  system  is 
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The  transformation  matrix  from  the  fixed-plane 


T  = 

ep 


to  body-fixed  system  is 
0  0^1 


(3) 


2.2  Beacon  Position  and  Attitude  Navigation  Aided  by  Magnetometer 

As  shown  in  figure  2,  suppose  the  position  of  an  object  is  (xo,vo,zo)  in  3-D  free  space  and  the 
attitude  of  the  object  is  ( 6 ,  i//,  <p).  A  beacons  are  located  on  the  ground  at  known  positions 
(xi,yi,Zi),  i  =  serving  as  reference  points  for  the  position  and  attitude  navigation.  The 

beacons  emit  RE  signals  with  their  antenna  patterns  spanning  3-D  free  space.  The  N  signals 
transmitted  from  the  beacons  are  represented  by  the  unit  vectors  of  sN ,  each  working 

at  frequencies respectively.  The  unit  vector  is  referred  to  as  the  ith  AOA  vector. 

As  shown  in  figure  2,  in  the  Earth-fixed  coordinate,  the  elevation  angles  of  the  AOA  vectors  are 
0i,  fl2v.  tf/v,  and  the  azimuth  angles  of  the  AOA  vectors  are  b\,  bN.  On  the  object,  N  sets  of 

ULAs  are  equipped  to  receive  impinging  signals  transmitted  from  the  beacons.  In  order  to 
distinguish  the  N  signals,  these  ULAs  are  applied  with  different  work  frequencies  of 
corresponding  to  different  signals.  The  structures  of  the  equipped  ULAs  are  shown  in  figure  3. 
As  shown  in  figure  3,  in  the  body-fixed  coordinate,  the  elevation  angles  of  the  AOA  vectors  are 
a\,  «,y,  and  the  azimuth  angles  are  fi\,  [h,---,  Pn-  The  directivities  of  the  AOA  vectors  in  the 

Earth-fixed  coordinate  are  described  by  the  elevation  and  azimuth  angles  ( a,  A ),  while  the 
directivities  of  the  AOA  vectors  in  the  body-fixed  coordinate  are  described  by  the  elevation  and 
azimuth  angles  {(M,Pi),  where  i  =  1,  2 In  addition,  a  magnetometer  is  equipped  on  the  spin 
axis  of  the  object  to  measure  the  magnetic  field  vector  in  the  body-fixed  coordinate.  The  magnetic 
field  vector  in  the  Earth-fixed  coordinate  is  along  the  Z-axis  in  figure  1 . 
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Body -fixed 
coordination 


TZb 


N  sets  of  uniform  linear  arrays  (ULAs)  with  work 


A  magnetometer 
is  equipped  along 
the  spin  axis 


Figure  3.  The  ULAs  equipped  on  the  object  receive  the  impinging  signals. 
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The  voltage  measurements  from  the  N  sets  of  ULAs  are  exploited  by  a  2-D  DO  A  estimation 
algorithm,  obtaining  the  estimation  of  the  N  pairs  of  elevation  and  azimuth  angles  in  the 
body-fixed  coordinate:  (a2,/?2 ),...,  (aN,/3N) .  The  ith  pair  of  the  elevation  and  azimuth 

angles  (a;.,/?;.)  is  referred  to  as  the  DOA  of  the  ith  AOA  vector  in  the  body-fixed  coordinate. 
Thus,  the  ULAs  can  only  measure  the  AOA  vectors  in  the  body-fixed  coordinate.  In  the 
body-fixed  coordinate, 

st  =  (sina.  cos  /?  sin  ai  sin  (3i  cos  dt  )T  ,i = 1, 2, . . .,  N .  (4) 

On  the  other  hand,  in  the  Earth-fixed  coordinate, 

S;  =  (sin  at  cos  bt  sin  at  sin  bt  cos  at  )T ,  i  =  1, 2, . . .,  N .  (5) 


A  relationship  between  the  AOA  vectors  in  the  body-fixed  coordinate  and  in  the  Earth-fixed 
coordinate  is  described  as  follows.  When  the  transformation  relationship  between  the  Earth- 
fixed  and  body-fixed  coordinates  is  applied  in  equation  1 , 
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for  i  =1,  2,. ..,7V.  Thus,  we  have 


u  =  Hv , 


where 
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A  transformation  of  equation  6  is 

^sin  at  cos  bt  ^  f 
sin  aj  sin  bi 
v  cos  at  j 

where  i  =  1,  2 


C\j/Ce 


c\i/ses<t> 


'svc</> 


s¥ce 

Sy/S6S<f>  Cy/C<fi 


\c¥sec<t>  si//s<t 


Sy/SeC<f> 


cvsi 


-sc 


ces<t> 
ceC(j>  J 


-V  ■ 


V 


sin  ai  cos  /?. 

/V 

sin  ai  sin  /?. 
cos  a 


J 


(6) 


(7) 

(8) 
(9) 


(10) 


(11) 


6 


2.3  Magnetometer  Model 

The  magnetometer  is  a  device  that  produces  a  voltage  output  proportional  to  the  applied 
magnetic  field  in  the  direction  of  the  magnetometer’s  sensitive  axis.  The  magnetometer  is  ideal 
for  broad  applications  because  it  is  very  rugged  and  low  cost.  Let  the  sensitive  axis  be  a  unit 
vector  i,  and  the  local  magnetic  field  vector  is  B.  Then  the  measurement  of  an  ideal 
magnetometer  is  the  inner  product  of  i  and  B 


Mideal  =  i  B  (12) 

When  the  scale  and  shift  influences  of  the  analog  circuitry  are  considered,  the  actual 
measurement  of  the  magnetometer  is 

M  =  s(i-B)  +  b,  (13) 

where  s  is  the  scale  factor  and  b  is  the  bias  offset.  If  the  included  angle  between  i  and  B  is  <9, 
equation  13  is  rewritten  as 


where  B  =  |B|.  Thus, 


M  =  sBcosO  +  b  , 


<9  =  cos  1 


f  M-b^ 

v  sB  , 


(14) 

(15) 


In  general,  the  SNR  of  the  magnetometer  is  very  high.  However,  some  amount  of  noise  is 
always  present,  which  is  modeled  as  white  Gaussian  noise  (WGN).  Therefore,  the  measurement 
of  the  magnetometer  is  adjusted  as 

M  =  sBcosO  +  b  +  riM,  (16) 

where  «t/is  WGN  with  zero  mean  and  a  variance  of  a  m- 


2.4  Iterative  Position  and  Attitude  Estimation 

As  described  in  section  2.2,  the  proposed  navigation  system  measures  one  magnetic  field  vector 
and  N  AOA  vectors.  Based  on  the  measurement  vectors,  it  is  proper  to  ask  if  these  are  sufficient 
to  estimate  either  the  position  or  the  attitude.  Consider  the  position  estimation  first.  It  will  be 
proven  in  section  3.2  that  three  linearly  independent  vectors  in  the  Earth-fixed  coordinate  with 
reference  points  are  necessary  and  sufficient  to  determine  the  position  of  an  object  in  3-D  free 
space.  Since  no  reference  point  is  associated  to  the  magnetic  field  vector,  it  does  not  contribute 
to  the  estimation  of  position.  Thus,  the  N  AOA  vectors  must  be  used  in  the  resection  algorithm 
to  estimate  the  position  of  the  object  (7).  As  mentioned  in  section  2.2,  the  ULAs  equipped  on  the 
object  can  only  measure  the  AOA  vectors  in  the  body-fixed  coordinate.  In  order  to  obtain  the 
AOA  vectors  in  the  Earth-fixed  coordinate,  the  elevation  and  azimuth  angles  (a^b,)  need  to 
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be  estimated  from  equation  11.  In  equation  1 1 ,  the  attitude  parameters  ( 9 ,  if/,  <p)  are  needed  to 
calculated  (a,  Aj-  Thus,  the  position  parameters  cannot  be  estimated  alone  without  attitude 
estimation. 

Next,  consider  the  attitude  estimation.  It  has  been  proven  that  two  pairs  of  linearly  independent 
vectors  are  necessary  and  sufficient  to  determine  the  attitude  of  an  object  (21).  That  means  we 
have  to  know  the  two  linearly  independent  vectors  in  both  the  Earth-fixed  coordinate  and  the 
body-fixed  coordinate.  As  described  in  section  2.2,  the  magnetic  field  vector  is  known  in  both 
coordinates,  thus  it  provides  one  pair  of  vectors  for  the  attitude  estimation.  In  order  to  estimate 
the  attitude  parameters,  we  need  to  find  another  pair  of  linearly  independent  vectors  from  the  N 
AOA  vectors.  Although  the  ULAs  measure  the  AOA  vectors  in  the  body-fixed  coordinate,  we 
cannot  obtain  the  AOA  vectors  in  the  Earth-fixed  coordinate  without  the  position  of  the  object. 
Thus,  we  cannot  find  another  pair  of  linearly  independent  vectors  without  position  estimation.  In 
an  attitude  estimation  system,  position  information  is  usually  obtained  by  GPS  (18).  Based  on 
the  previous  analysis,  the  position  or  attitude  cannot  be  estimated  alone. 

Given  that  the  position  and  attitude  must  be  estimated  jointly,  the  next  question  to  address  is  if 
the  position  and  attitude  can  be  estimated  simultaneously.  The  magnetometer  measurement  is 
shown  in  equation  16.  The  ULA  measurements  are  described  in  equation  6,  which  can  be 
rewritten  as 
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where  norm;  =  { (x,  -xo)  +  (>’,  -  Vo)  +  (z,  -  zq)  }  .  Thus,  equations  16  and  17  consist  of  a 

T 

system  of  equations,  where  m  =  (xo,yo^o,  9,  '/A  <p)  are  the  unknown  position  and  attitude 
parameters  to  be  estimated.  Equations  16  and  17  are  nonlinear  equations,  where  the  unknown 
parameters  are  coupled  together.  In  the  simultaneous  position  and  attitude  estimation  process, 
the  parameter  vector  m  must  be  estimated  using  recursive  algorithms,  such  as  a  nonlinear  least 
squares  estimation  algorithm.  Our  extensive  simulations  show  that  these  simultaneous  position 
and  attitude  estimation  algorithms  are  not  robust.  If  the  SNR  of  the  ULAs  is  smaller  than  25  dB, 
the  algorithm  exploiting  four  beacons  does  not  converge  with  very  high  probability. 

While  the  simultaneous  position  and  attitude  estimation  is  not  robust,  iterative  position  and 
attitude  estimation  algorithms  are  proposed  next  based  on  beacons  and  a  magnetometer. 

Suppose  the  initial  attitude  of  the  object  before  launch  is  known  as  (9* ,i//* ,tp*).  At  the  beginning 
of  the  estimation  process,  the  initial  attitude  parameters  of  Up,  if/,  9)  are  used  to  solve  the 
elevation  and  azimuth  angles  (ai,bi)  according  to  equation  9.  Based  on  the  estimation  results  of 
(dl,bj) ,  the  position  of  (xo,yo,zo)  can  be  subsequently  estimated  based  on  the  3-D  AOA  location 
algorithm  proposed  in  section  3.2.  Based  on  equations  16  and  17  and  the  position  estimation 
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results,  the  attitude  parameters  of  (6,  i//,  <p)  can  be  updated  by  the  nonlinear  estimation  approach 
proposed  in  section  4.2.  Then  the  current  attitude  estimation  results  will  be  exploited  to  update 
(af,4)  i11  the  next  iteration.  The  scheme  of  the  iterative  position  and  attitude  estimation 
algorithms  is  illustrated  in  figure  4.  The  details  of  the  position  and  attitude  estimation  algorithm 
are  presented  in  sections  3  and  4. 


Figure  4.  Iterative  position  and  attitude  estimation  algorithm  based  on  beacons 
and  a  magnetometer. 

In  section  3.2,  it  is  shown  that  three  AOA  vectors  are  necessary  and  sufficient  to  determine  the 
position  of  the  object.  In  the  appendix,  it  is  shown  geometrically  that  two  AOA  vectors  are 
necessary  and  sufficient  to  determine  the  attitude  of  the  object.  Therefore,  in  order  to  estimate 
both  position  and  attitude,  the  proposed  algorithms  use  three  ground-based  beacons.  Since  the 
magnetometer  has  very  high  SNR,  an  aiding  magnetometer  measures  an  additional  magnetic 
field  vector  to  improve  the  performance  of  the  position  and  attitude  estimation. 


3.  Position  Estimation  Based  on  3-D  AOA  Vectors 


3.1  2-D  AOA  Location  Algorithm 

In  this  section,  the  2-D  AOA  location  algorithm  will  be  first  summarized  and  exploited  as  the 
foundation  of  the  proposed  3-D  AOA  location  algorithm.  The  2-D  AOA  position  algorithm, 
referred  to  as  the  resection  algorithm,  was  proposed  by  Rappaport  et  al.  ( 4 )  and  McGillem  and 
Rappaport  (7).  The  coordinates  for  the  analytic  geometry  solution  of  the  AOA  position  location 
are  shown  in  figure  5,  where  the  position  of  points  A,  B,  and  C  are  (xi,vi),  (X2,>’2),  and 
respectively.  The  angle  between  AP  and  BP  is  y\.  The  angle  between  BP  and  CP  is  yi-  The 
angle  between  AB  and  the  horizontal  direction  is  z\.  The  angle  between  BC  and  the  horizontal 
direction  is  r 2-  Let  the  position  of  P  be  (xoj'o),  which  is  calculated  based  on  the  following  known 
parameters: 
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Figure  5.  Coordinates  for  the  analytic  geometry  solution  of 
the  2-D  AOA  position  location. 
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where 
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x0  =  my0  +  n  , 
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xN  =  x,  -Rlsin(Tl  -/,  )  , 
yN  =  Ji  ~Rlcos(rl  -yx)  , 
xM  =  x2  -  R2  sin(r2  -y2)  , 
yM  -x2-R2  cos(r2  -y2)  > 

R  - 

1  2  sin  yx 
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and 


BC 


R2 

2  sin  y2 

For  the  case  of  x0  =  my0  +  n ,  equations  18  and  19  maybe  equivalently  expressed  as 

2m  yN  +  2xn  +  2m  n 


(27) 


x0  =• 


1  +  m 


•  — x. 


(28) 


and 


y0  =mx0-n 


(29) 


where  m  =  —  and  n  =  —  • 
m  m 

3.2  Position  Estimation  Based  on  3-D  AOA  Measurements 

In  this  section,  a  3-D  AOA  location  algorithm  is  generalized  from  the  2-D  AOA  location 
algorithm  described  in  section  3.1.  It  is  noted  that  the  resection  algorithm  requires  at  least  three 
AOA  vectors  to  solve  for  the  location  in  two  dimensions.  Suppose  three  beacons  at  known 
positions  are  available.  It  will  be  shown  in  the  following  section  that  the  three  beacons  are 
sufficient  to  solve  for  the  location  in  three  dimensions,  if  the  attitude  of  the  object  is  assumed  to 
be  known  as  {6,\j/,¥) .  In  order  to  estimate  the  position  of  the  object  in  3-D  free  space,  the  3-D 
coordinates  are  first  projected  into  the  2-D  X-Y  plane,  as  illustrated  in  figure  6,  where  the  three 
beacons  are  located  at  points  A(xi,yi),  B(x2,y2),  and  C(x 3,73),  respectively.  The  position  of  the 
object  in  the  X-Y  plane  is  P(xo,yo).  The  included  angle  between  AP  and  BP  is  \b2  -  b\\,  and  the 
included  angle  between  BP  and  CP  is  \b2  -  b2\,  where  b\,b2,  and  b2  are  the  azimuth  angles  of 
,  s2,  and  ,?3  in  the  Earth-fixed  coordinate,  respectively.  However,  when  the  position  of  the 
object  is  unknown,  b\,  b2,  and  b2  cannot  be  determined.  Therefore,  b\,  b2,  and  b2  need  to  be 
estimated  from  the  attitude  estimation  results.  According  to  equation  1 1,  we  have 
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Figure  6.  The  3-D  coordinates  are  projected  into  the  2-D 
X-Y  plane. 


where  i  =  1,  2,  3.  In  equation  30,  6,  \j/ ,  and  (f)  are  the  attitude  estimation  results,  and  ai  and  /?, 
are  the  estimation  of  elevation  and  azimuth  angles  of  si  in  the  body-fixed  coordinate,  obtained 

from  the  fast  2-D  DOA  estimation  algorithm  described  in  section  4.1.  According  to  equation  30, 
a,  and  bj  are  estimated  as  follows: 

a.  =  cos'1  (4)  (31) 

and 


tan -\sjsn)  : 
bt=[  tan"1  (si2/sn)  +  2n  : 


^_>0®— ^>0 
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— — —  <  0  ®  — — —  >  0 
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tan  1  (si2  /sn)  +  t:  : 


otherwise 


These  parameters  are  applied  for  the  resection  algorithm  described  in  section  3.1  to  estimate  x0 


and  y0 ,  where  yl  = 
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,  r,  =  tan 
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and  t2  =  tan 
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In  addition,  the  altitude  of  the  object  zq  may  be  estimated  from  the  geometric  relationship 
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(33) 


tan2  a  iJ*o-xl)2+(yo-yl)2 

(t0-z,)2 


(Xg  -X,)2  +(>>0  -J,)2 

tan2  a. 


where  i=  1,  2,  3.  When  the  mean  estimation  is  applied,  z0  can  be  estimated  as 
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Therefore,  three  beacons  are  necessary  and  sufficient  to  estimate  the  position  of  the  object  in  3-D 
free  space. 


4.  Attitude  Estimation  Algorithm  Based  on  2-D  DOA  Estimation 


4.1  A  Fast  2-D  DOA  Estimation  Algorithm 

Several  2-D  DOA  estimation  algorithms  have  been  presented  in  the  literature.  Maximum 
likelihood  methods  with  high  computational  complexity  were  first  proposed  (10, 11).  MUSIC 
and  ESPRIT  algorithms  exploited  either  eigen  value  decomposition  or  singular  value 
decomposition  to  obtain  high  resolution  in  DOA  estimation  (12-15).  Wu  et  al.  (16)  proposed  a 
2-D  DOA  estimation  algorithm  based  on  propagator  method.  Recently,  Tayem  and  Kwon  (1 7) 
proposed  a  computationally  efficient  azimuth  and  elevation  angle  estimation  algorithm  with  no 
failure  and  no  eigen  decomposition.  The  computational  efficiency  and  accuracy  of  this 
algorithm  are  suitable  for  the  real-time  estimation  of  a  rapidly  moving  object.  This  fast  2-D 
DOA  estimation  algorithm  is  summarized  in  the  appendix.  It  will  be  exploited  to  formulate  the 
position  and  attitude  estimation  algorithms. 

Figure  7  shows  the  proposed  array  configuration  consisting  of  three  ULAs  with  interspacing  d 
equal  to  a  half  wavelength  of  incident  signals.  The  three  ULAs  consist  of  N,  N+l,  and  N 
elements,  respectively.  The  first  and  third  ULAs  are  referred  to  as  subarrays  Z  and  W, 
respectively.  The  first  and  last  N  elements  in  the  second  ULA  are  referred  to  as  subarrays  X  and 
Y,  respectively.  Thus,  subarrays  X. ,  Y,  Z,  and  W  consist  of  N  elements.  Suppose  there  are  K 

T 

narrow-band  sources,  s (t)  =  [s\(t),S2(t),...,SK  (t)]  >  with  the  same  wavelength  X  impinging  on  the 
arrays.  The  received  signals  at  the  subarrays  X,  Y,  Z,  and  W  are 
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Figure  7.  The  proposed  array  configuration  equipped  on  the 
object. 


x(0=  As(0  +  nx(t) , 
y(t)=  AOys(t)  +  nv(0  , 
z(t)=  A<Dzs(t)  +  n z(t)  , 
w (0=  A<Dws(t)  +  nH(f) , 

t=l,2,...,L,  (36) 

where 
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(37) 
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where  k=  1,  2 Let  g(t)  =  [xr(t),  yT(t),  z T(t),  wr(t)] 7  denote  a  AN  x  1  snapshot  data  vector 
with  t=  1,  2 Let  F  =  [g(l),  g(2),...,  g(L)]  denote  a  4/V  x  L  data  matrix  consisting  of  L 
snapshots.  The  partition  of  F  is  F  =  [F/,  F27']7',  where  Fi  and  F2  are  submatrices  with 
dimensions  of  K  x  L  and  (AN  -  K)  x  L,  respectively.  Let  P  =  ( F|  F 1  /7)_  1 F 1  ¥2'  denote  the  K  x 
(AN-  K)  propagator  estimate  matrix  based  on  the  data  matrix  F.  The  partitions  of  P  are 
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4.2  Attitude  Estimation  Algorithm  Based  on  Fast  2-D  DO  A  Estimation 

In  this  section,  an  attitude  estimation  algorithm  is  proposed  based  on  the  fast  2-D  DOA 
estimation  described  in  section  4. 1 .  Suppose  one  magnetometer  is  equipped  on  the  spin  axis  of 
the  object.  According  to  equation  15,  and  assuming  b  =  0  and  sB  =  1 ,  the  pitch  angle  (assuming 
a  vertical  magnetic  field  vector)  is  estimated  as 

#  =  -sin_1(M)  ,  (44) 

where  M  is  the  measurement  of  the  magnetometer. 

On  the  other  hand,  it  has  been  proven  that  two  pairs  of  linearly  independent  vectors  are  necessary 
and  sufficient  to  determine  the  attitude  of  the  object  (21).  That  means  that  given  the  position  of 
the  object,  two  ground-based  beacons  are  necessary  and  sufficient  to  determine  the  attitude.  The 
appendix  gives  a  geometric  explanation  for  this.  Our  proposed  navigation  system  includes  three 
beacons  that  are  redundant  to  estimate  the  attitude.  Corresponding  to  the  three  beacons,  three 
sets  of  ULAs  are  equipped  on  the  object  to  receive  impinging  signals.  As  described  in  section 
3.2,  the  current  position  parameters  have  already  been  estimated  as  (x0,  j>0,z0).  Let  the  positions 

of  the  three  beacons  be  (xi,yi,zi),  (x2,y2,zi),  and  (*3,^3, Z3),  respectively.  Based  on  equation  17,  we 
have 
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C  SiS.  -sc.  s  SAS.  +c  c,  c„s, 

y/  0  <p  V  (p  y/  0  (p  y/  <p  0  <p 

(}’,  -y0)/  norm, 

K  cos  6t.  j 

.  c  s~c,  +  s  s ,  s  s~c,  -  c  S,  C~C,  J 

V  ¥  9  <p  y/  <p  y/  0  <p  y/  </>  6  <p  J 

.  (z,-Zo)/norm, 

where  norm,  =  {(x,  -x0)2  +  (yj  - y0)2  +  (z;  -z0)2 }  1/2  and  i  =  1,  2,  3.  Thus,  we  have 


(45) 


u  =  Hv , 


(46) 
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where 


u  =  [u[,U2,...,u^,]rand---ui  =  (-sina;.  cos/^-sin^.  sin/j^-cosa,)7  ,  (47) 

v  =  [v^V2,...,v£fand"-vi  =  (sina(  cos4sina;  sin&(.,cosa;)r,/  =  1,2,..., N  ,  (48) 


and 


H 


■%  0  0  ^ 

0  X„  0 


0  0  T 


(49) 


eb  J 


x  x  ^  y .  —  y  z .  —  z 

Let  sn  =  J — _° ,  si2  =  J — _°  ,  and  4  =  J — 4  .  From  the  first  equation  of  equation  45,  we 


nornii 


nornii 


nornii 


have 


cos  (y/.-r.)  = 


sin  ai  cos  [ii  +  s-s 


do 


(50) 


where  y  can  be  estimated  as 


tan-1  (4/4) 


Sil  >00  ,  ^  r  >  0 


'  7^1  M 


2  ~2 
i+4 


Yi=(  tan"  (4/4)  +  2tt  : 


S‘2  <00  ,  5/2  r<  Q 


[7, 2  /v2  /~4 

V^'l+^2 


2  +  4 

1  T  A'2 


(51) 


tan"  (4/4)  + ^  : 


otherwise 


Since  the  range  of  y/.  -  y.  is  unknown,  yi  cannot  be  uniquely  estimated  from  equation  50.  The 
estimation  ij/i  from  the  last  iteration  then  provides  an  estimate  of  the  range  of  y/i  -  yt .  Let  4  be 
the  estimation  result  of  \|/(  in  the  4  iteration.  Then  in  the  k+  1th  iteration, 

y(.  -  271  +  A 
y.-A 
Y,  +  A 
y;  +  271 -A 

where 


V,  -y,  e [-271,-71] 

k  "  ~[-n,0] 

[0,7l] 


Vi 


Vi 


"Yi 

■Y/ 


4 -y,  £[71,271] 


(52) 
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A  =  cos 


-i 


f  sin  ai  cos  /?,.  +  s-si3  ^ 


Averaging  over  the  three  sets  of  estimations,  we  have 

-  I  v  « 

v  =  -2_tYi  . 

2  1=1 

From  the  second  and  third  equations  of  equation  45,  we  have 

sin  ai  cos  /?  =  ml  sin  (f>  +  m2  cos  <j> 
cos  cci  =  m3  sin  (j)  +  m4  cos  (f> 


(53) 


(54) 


(55) 


where 


m ,  =  sin  a,,  cos  hc-s-  +  sina,  sin  b.s-s-  +  cos  a,c- 

1  L  1  Y  U  1  1  r  U  1  U 

/v  /v 

m2  -  sina,.  sin  -sina,  cos  bts^ 
m3  -  sina,.  cos b{s^  -  sina,.  sin b.c^ 
m4  -  sina,.  cos +sina,.  sin b^s^s-  +cos atc~ 


Thus,  for  each  i,  sin  </>t  and  cisft  can  be  solved  as 


Therefore, 


.  -  -  mA  sin  a,  sin  B.  -  m,  cos  a, 

sin  (pi  = - - - - — - - - 

mlm4  -  m2m3 

?  -  m3  sin  a,  sin  B.  -  m ,  cos  <x 

cos  <pl  = - - - - - — - - - - 

m2m2  -  mlm4 


I  tan"1  (sin  0.  /cos  <j>.) 

/\  /  /\  /  /v 

$  =  (  tan  (sin  <f)t  /  cos  (j)i )  +  2k 
\  tan  ~\snjsn)  +  7u 


sin^.  >  0  ©  cos^,.  >  0 
sin^,.  <  0®  cos^,.  <  0 
otherwise 


Applying  an  averaging  operation  over  the  three  sets  of  estimations,  we  have 

1  3 

/V  I  — - ,  /V 


(56) 


(57) 

(58) 


(59) 


(60) 
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5.  Simulations 


5.1  Performance-Enhancement  Techniques 

In  section  4.2,  equations  54  and  60  are  intended  to  give  good  estimations  of  the  yaw  and  roll 
angles,  respectively.  However,  when  the  denominators  in  equations  50  and  57-59  are  small,  the 
errors  of  the  estimations  are  magnified.  In  addition,  in  the  iterative  position  and  attitude 
estimation  algorithms,  the  error  may  be  transferred  between  each  other  and  accumulated.  In 
order  to  further  improve  the  estimation  performance,  a  recursive  median  filter  is  applied  in  the 
estimation  process  to  remove  large  deviations  from  the  estimation  results  (32).  The  output  of  the 
recursive  median  filter  with  window  width  equal  to  2N  +  1  is 

xM(t)  =  median  [xM(t  -  NAt),...,xM(t  -  At),x(t),x(t + At),...,xM(t + NAt)]  ’ 

where  x  is  the  arbitrary  estimated  parameter  and  At  is  the  sampling  interval.  In  the  recursive 
median  filter,  the  prior  outputs  are  used  in  the  current  filter  step.  As  shown  in  equation  61, 
xM  (t  -  NAt),...,  xM  (t  -  At)  are  prior  outputs.  Since  the  SNR  of  the  magnetometer  is  usually  very 

high,  the  recursive  median  filter  is  not  applied  to  the  estimation  of  the  pitch  angle. 

5.2  Simulations  of  Iterative  Position  and  Attitude  Estimation  Algorithms 

The  proposed  iterative  position  and  attitude  estimation  algorithms  are  simulated  using  a 
gun-launched  object.  In  these  simulations,  the  object  is  launched  with  an  initial  pitch  angle 
90  =  45°,  initial  yaw  angle  i//o  =  0°,  initial  roll  angle  tpo  =  0°,  and  initial  position  at  (0,  0,  0).  The 
trajectory  of  the  object  is  a  standard  parabola.  The  range  of  the  trajectory  is  3  km  and  the 
traveling  time  is  2  s.  Three  ground-based  beacons  are  located  at  (-1500,  3000,  0),  (4000,  1000, 
0),  and  (6000,  -4000,  0),  respectively.  The  work  frequencies  of  the  three  beacons  are 
/i  =  10  GHz ,f2  =  5  GHz,  and  =  7.5  GHz,  respectively.  All  the  subarrays  of  the  ULAs  equipped 
on  the  object  consist  of  N=  4  elements.  Assume  the  noise  on  the  ULAs  is  WGN,  with  SNR  =  20 
dB.  The  noise  on  the  magnetometer  is  also  WGN,  with  the  variance  o2 M=  10  6. 

The  relative  locations  between  the  trajectory  of  the  object  and  the  three  beacons  are  illustrated  in 
figure  8.  Recursive  median  filters  with  window  width  equal  to  7  are  applied  to  the  estimations  of 
y/,  cp,  xo,yo,  and  zq.  The  simulation  results  are  shown  in  figures  9-14,  corresponding  to  the 
estimations  of  coordinates  on  the  X-axis,  Y-axis,  Z-axis,  and  pitch  angle,  yaw  angle  and  roll 
angle,  respectively. 
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Figure  8.  Relative  locations  between  the  trajectory  of  the  object  and  the 
three  beacons. 


Figure  9.  Simulation  result  for  the  estimation  of  x0.  SNR  =  20  dB. 
MSE  =  2.63  x  104. 
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Figure  10.  Simulation  result  for  the  estimation  of  y0.  SNR  =  20  dB. 
MSE  =  1.11  x  104. 
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Figure  1 1 .  Simulation  result  for  the  estimation  of  z0.  SNR  =  20  dB. 
MSE  =  2.37  x  105. 


Figure  12.  Simulation  result  for  the  estimation  of  pitch  angle.  SNR 
=  20dB.  MSE=  1.31  x  10'6. 
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Figure  13.  Simulation  result  for  the  estimation  of  yaw  angle. 
SNR  =  20  dB.  MSE  =  3.37  x  10'4. 
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Figure  14.  Simulation  result  for  the  estimation  of  roll  angle. 

SNR  =  20  dB.  MSE  =  2.97  x  10'1. 

In  figures  9-14,  the  MSE  of  x0  is  2.63  x  104,  the  MSE  of  y0  is  1.1 1  x  104,  the  MSE  of  z0  is 
2.37  x  105,  the  MSE  of  6  is  1.31  x  10“6,  the  MSE  of  y>  is  3.37  x  10  4,  and  the  MSE  of  $  is 
2.97  x  10_1.  The  recursive  median  filters  successfully  remove  some  large  deviations  from  the 
estimation  results.  However,  it  is  noted  that  there  are  still  some  singular  points  in  figures  9-11, 
13,  and  14,  which  result  in  the  large  MSEs  of  Jc0 ,  y0 ,  and  z0 .  There  are  two  reasons  for  the 
singularity.  First,  the  denominators  in  equations  57-59  are  close  to  zero.  Second,  the  use  of  ft 
to  guess  the  range  of  (j)l  +  -  y,  in  equation  52  can  sometimes  be  inaccurate.  In  addition,  the 
period  of  these  deviations  is  longer  than  the  window  period  of  the  used  recursive  median  filters. 
Therefore,  the  recursive  median  filters  cannot  remove  these  deviations.  In  our  future  research, 
some  approaches  will  be  proposed  to  eliminate  the  singular  points  of  the  iterative  position  and 
attitude  estimation  algorithms,  which  are  out  of  the  scope  of  this  report. 


6.  Conclusion 


This  report  proposes  robust  low-cost  and  real-time  position  and  attitude  estimation  algorithms 
based  on  ground-based  beacons  and  magnetometers.  Three  sets  of  ULAs  equipped  on  the  object 
with  distinguishable  work  frequencies  receive  signals  transmitted  from  three  ground-based 
beacons.  The  directions  of  the  AOA  vectors  are  estimated  by  fast  2-D  DOA  estimation 
algorithm.  Based  on  the  prior  knowledge  of  initial  attitude  parameters,  the  position  of  the  object 
is  estimated  based  on  a  3-D  AOA  location  algorithm.  Based  on  the  position  estimation  results, 
current  attitude  parameters  are  updated  and  used  to  estimate  the  position  parameters  in  the  next 
estimation  cycle.  A  recursive  median  filter  is  applied  to  remove  large  deviations  from  the 
estimation  results  and  improve  the  performance.  Simulations  show  that  the  proposed  low-cost 
and  real-time  algorithms  successfully  estimate  the  position  and  attitude  most  of  the  time.  The 
algorithms  developed  for  this  technique  are  applicable  to  the  special  case  of  a  perfectly  vertical 
magnetic  field,  thus  additional  angular  transforms  would  need  to  be  introduced  for  solutions  to 
more  general  magnetic  orientation  cases. 
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Appendix.  Fast  Two-Dimensional  Direction  of  Arrival  Estimation  Algorithm 


Consider  one  impinging  signal  represented  by  the  vector  s  that  is  received  by  a  set  of  parallel 
uniform  linear  arrays  (ULAs)  equipped  on  the  object,  as  shown  in  figure  A-l,  and  assume  that 
the  position  of  the  object  is  known. 


Vb 

> 


Body- fixed 
coordination 


T  Zb 


Three  parallel  uniform 
linear  arrays  (ULAs) 


0  :  • 


4  P 


Figure  A-l.  One  impinging  signal  is  received  by  the 

system  of  three  parallel  ULAs  equipped  on  the 
object. 


When  the  fast  two-dimensional  direction  of  arrival  estimation  algorithm  is  used,  the  elevation 
angle  a  and  the  azimuth  angle  [3  are  estimated  as  a  and  [3 ,  respectively.  According  to  the 
cosine  theorem,  the  included  angle  between  the  Xf,  axis  and  ?  is 

oox  =  cos  _1  (cos  / 3  cos(90°  -a))  =  cos  _1  (cos  / 3  sin  a).  (A- 1 ) 

The  direction  of  s  in  the  Earth- fixed  coordinate  can  be  determined  by  the  positions  of  the 
beacon  and  the  object.  Based  on  the  signal  direction  s  and  the  included  angle  oo ,  a  cone  with  an 
axis  of  s  can  be  determined,  on  whose  surface  infinite,  body- fixed  coordinates  can  be  found  to 
satisfy  the  estimated  elevation  and  azimuth  angles.  Figure  A-2  shows  two  sets  of  body-fixed 
coordinates  ( Xb ,  Yb ,  Zh )  and  ( Xb ,  Yb ,  Zh)  satisfying  the  given  conditions.  Therefore,  given 

the  position  of  the  object,  one  angle  of  arrival  (AO A)  vector  is  not  sufficient  to  determine  the 
attitude  of  the  object. 
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Figure  A-2.  One  impinging  signal  determines  a  cone, 
on  whose  surface  infinite  body-fixed 
coordinates  can  be  found  to  satisfy  the 
estimated  elevation  and  azimuth  angles. 


If  two  impinging  signals  are  transmitted  with  direction  vectors  of  s1  and  s2 ,  two  cones  with  axes 
of  and  s2  are  determined  by  the  included  angle  between  the  Xb  axis  and  the  vector  directions. 
Let  the  centers  of  the  undersurfaces  of  the  two  cones  be  0\  and  Of-  There  are  at  most  two 
intersection  lines  between  the  two  cones,  OA  and  OB  ,  as  shown  in  figure  A-3.  Let  the 
estimated  elevation  and  azimuth  angles  from  the  first  impinging  signal  be  ( ax ,  )  and  the 

estimated  elevation  and  azimuth  angles  from  the  second  impinging  signal  be  ( a2 ,  /?2 ).  In  figure 
A-3,  assume  that  the  body- fixed  coordinate  ( X h ,  Yb ,  Zh )  on  the  intersection  line  OA  satisfies 
the  elevation  and  azimuth  angles  ( ax ,  /?, )  and  ( a2 ,  fi2 )  with  respect  to  and  s2 ,  respectively. 
On  the  intersection  line  OB  ,  body- fixed  coordinate  ( X'b ,  Yh ,  Zh )  satisfies  the  elevation  and 
azimuth  angles  ( ay ,  /?, )  with  respect  to  .  Body- fixed  coordinate  ( X"h ,  Yh  ,  Z"h)  satisfies  the 
elevation  and  azimuth  angles  ( a2 ,  [i2 )  with  respect  to  s2 ,  where  Xb  overlaps  Xb .  Thus,  only  ( 
Xb ,  Yb,Zb )  satisfies  all  of  the  given  conditions.  In  conclusion,  given  the  position  of  the  object, 
two  linearly  independent  AO  A  vectors  are  sufficient  to  determine  the  attitude  of  the  object. 
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